
#include"encoder.h"
#include"kinematics.hpp"
#include"motor.h"
#include"pid.hpp"

float ctrlrate=0.5;
unsigned long lastctrl;
double x=0,y=0,theta=0;

float KP = 10.0;
float KI = 0.05;
float KD = 25.05;

MPID PIDA(encA,KP,KI,KD,true);
MPID PIDB(encB,KP,KI,KD,false);
MPID PIDC(encC,KP,KI,KD,true);
MPID PIDD(encD,KP,KI,KD,false);

float wA,wB,wC,wD;
unsigned long change_motor_action_time = 0;
unsigned long calculate_time_period = 0;
unsigned int car_move_status = 0;

void set_vel(float speed_vx, float speed_vy, float speed_vw){
  InverseKinematic(speed_vx, speed_vy, speed_vw, pwma, pwmb, pwmc, pwmd);
  // Serial.println("---------chuandi-------");
  // Serial.print(pwma);
  // Serial.print(" : ");
  // Serial.print(pwmb);
  // Serial.print(" : ");
  // Serial.print(pwmc);
  // Serial.print(" : ");
  // Serial.println(pwmd);
  // Serial.println("---------chuandi-------");
}

void cmdVelCb(){
  PIDA.tic();
  MotorA(PIDA.getPWM(pwma));
  wA=PIDA.getWheelRotatialSpeed();
  PIDA.toc();

  PIDB.tic();
  MotorB(PIDB.getPWM(pwmb));
  wB=PIDB.getWheelRotatialSpeed();
  PIDB.toc();

  PIDC.tic();
  MotorC(PIDC.getPWM(pwmc));
  wC=PIDC.getWheelRotatialSpeed();
  PIDC.toc();

  PIDD.tic();
  MotorD(PIDD.getPWM(pwmd));
  wD=PIDD.getWheelRotatialSpeed();
  PIDD.toc();

  lastctrl=millis();
}

void setup()
{
  delay(2000);
  Serial.begin(115200);
  IO_init();
  PIDA.init();
  PIDB.init();
  PIDC.init();
  PIDD.init();
  set_vel(0.0, 0.0, 0.0);
  change_motor_action_time = millis();
  lastctrl=millis();
}

void loop(){
  // set_vel(0.0,  0.0,  0.0);
  cmdVelCb();
  float ax,ay,az,gx,gy,gz;
  delay(10);
  if( (millis() - change_motor_action_time) > 3000 ){
    car_move_status += 1;
    // Serial.println("------------------------------------------------------------------------------");
    change_motor_action_time = millis();
    switch(car_move_status){
      case 1: {
        // Serial.println("forward:");
        set_vel(-0.1,  0.0,  0.0);}
        break;                                          //forward
      case 2: {set_vel( 0.0,  0.0,  0.0);} break;       //stop
      case 3: {
        // Serial.println("back:");
        set_vel( 0.1,  0.0,  0.0);} 
        break;                                          //back
      case 4: {set_vel( 0.0,  0.0,  0.0);} break;       //stop
      case 5: {
        // Serial.println("left:");
        set_vel( 0.0,  0.0,  0.5);} 
        break;                                          //left
      case 6: {set_vel( 0.0,  0.0,  0.0);} break;       //stop
      case 7: {
        // Serial.println("right:");
        set_vel( 0.0,  0.0, -0.5);} 
        break;                                          //right
      case 8: {set_vel( 0.0,  0.0,  0.0);} break;       //stop
      case 9: {set_vel( 0.0,  0.1,  0.0);} break;    //left_translation
      case 10: {set_vel( 0.0,  0.0,  0.0);} break;    //stop
      case 11: {set_vel( 0.0, -0.1,  0.0);} break;    //right_translation
      case 12: {set_vel( 0.0,  0.0,  0.0);} break;    //stop
      default: break;
    }
    if(car_move_status >= 12){
      car_move_status = 0;
    }
  }
  // if((millis()-lastctrl)>1000*ctrlrate){
  //   STOP();
  // }

}
